#include "modbus_handler.h"
#include "create_modbus_custom_slave.h"
#include "../TableInterface/upper_table_interface.h"
#include "../dataBase/logRecordFunction.h"
#include "../dataBase/errWaveFunction.h"
#include <stdlib.h>
#include <stdbool.h>
#include "../RealtimeWaveform.h"
#include "../debug.h"
#include <pthread.h>
#include <unistd.h>
#include <sys/syscall.h>
#include "../misc.h"
#include <errno.h>

extern bool glb_upper_connect_flag;
#define CUS_CONNECTION  1
#define local_server_port1 777
#define local_server_port2 888
#define SEND_BUF_LENGTH    1024
#define ARM_MAX_LENGTH     860

void* customModbus_server1_thread(void* arg){
    const ConfigData *config_data = get_config_data_instance();
    Misc_InitThread(__func__);
    create_customServer_connect(config_data->slave1.server_ip,local_server_port1);
    return NULL;
}

void* customModbus_server2_thread(void* arg){
    const ConfigData *config_data = get_config_data_instance();
    Misc_InitThread(__func__);
    create_realtimeWaveDataServer_connect(config_data->slave1.server_ip,local_server_port2);
    return NULL;
}

int create_customServer_connect(const char* server_ip, int port){
    modbus_t *ctx = NULL;

    int server_socket = -1;
    uint8_t query[MODBUS_TCP_MAX_ADU_LENGTH];
    int master_socket;
    int client_socket = -1;
    int rc;
    bool connected = false;
    fd_set ref_rdset;
    fd_set rdset;

    /* Maximum file descriptor number */
    int fdmax;

    //上下文获取
    ctx = modbus_new_tcp(NULL, port);


    server_socket = modbus_tcp_listen(ctx, CUS_CONNECTION);
    if (server_socket == -1) {
        fprintf(stderr, "Unable to listen TCP connection\n");
        modbus_free(ctx);
        return -1;
    }

    /* Clear the reference set of socket */
    FD_ZERO(&ref_rdset);
    /* Add the server socket */
    FD_SET(server_socket, &ref_rdset);

    /* Keep track of the max file descriptor */
    fdmax = server_socket;

    while(1) {
        rdset = ref_rdset;

        if (select(fdmax + 1, &rdset,NULL, NULL, NULL) == -1) {
            perror("Server select() failure.");
            if (server_socket != -1) {
                close(server_socket);
            }
            modbus_free(ctx);
        }

        /* Run through the existing connections looking for data to be
         * read */
        for (master_socket = 0; master_socket <= fdmax; master_socket++) {

            if (!FD_ISSET(master_socket, &rdset) ) {
                continue;
            }

            if (master_socket == server_socket) {
                /* A client is asking a new connection */
                socklen_t addrlen;
                struct sockaddr_in clientaddr;
                int newfd;
                if(client_socket != -1 && connected){
                    close(client_socket);
                    connected = false;
                    glb_upper_connect_flag = false;
                    printf("Disconnect old connections and establish new ones!\n");
                    /* Remove from reference set */
                    FD_CLR(client_socket, &ref_rdset);
                    
                    if (client_socket == fdmax) {
                        fdmax--;
                    }                    
                }
                             
                /* Handle new connections */
                addrlen = sizeof(clientaddr);
                memset(&clientaddr, 0, sizeof(clientaddr));
                newfd = accept(server_socket, (struct sockaddr *) &clientaddr, &addrlen); 
                if (newfd == -1) {
                    perror("Server accept() error");
                } else {
                    connected = true;
                    FD_SET(newfd, &ref_rdset);
                    if (newfd > fdmax) {
                        /* Keep track of the maximum */
                        fdmax = newfd;
                    }
                  
                    printf("New connection from %s:%d on socket %d\n",
                           inet_ntoa(clientaddr.sin_addr),
                           clientaddr.sin_port,
                           newfd);
                }
            } else {
                modbus_set_socket(ctx, master_socket);
                //rc = modbus_receive(ctx, query);
                int sockfd = modbus_get_socket(ctx);
                client_socket = sockfd; 
                rc = recv(sockfd, query,MODBUS_TCP_MAX_ADU_LENGTH,0);
                if (rc > 0) {
                    printf("[%s][%d]: data handle\n", __func__,__LINE__);
                    handle_client_request(ctx, query, rc);
                    
                } else if (rc == -1) {
                    /* This example server in ended on connection closing or
                     * any errors. */
                    glb_upper_connect_flag = false;
                    connected = false;
                    printf("Connection closed on socket %d\n", master_socket);
                    close(master_socket);

                    /* Remove from reference set */
                    FD_CLR(master_socket, &ref_rdset);
                    
                    if (master_socket == fdmax) {
                        fdmax--;
                    }
                }
            }
            usleep(10000); //10ms
        }
    }

    return 0;    
}

int create_realtimeWaveDataServer_connect(const char* server_ip, int port){
    modbus_t *ctx = NULL;
    int server_socket = -1;
    uint8_t query[MODBUS_TCP_MAX_ADU_LENGTH];
    int master_socket;
    int client_socket = -1;
    int rc;
    bool connected = false;
    fd_set ref_rdset;
    fd_set rdset;
    struct timeval timeout = {0,10000};

    /* Maximum file descriptor number */
    int fdmax;

    //上下文获取
    ctx = modbus_new_tcp(NULL, port);

    server_socket = modbus_tcp_listen(ctx, CUS_CONNECTION);
    if (server_socket == -1) {
        fprintf(stderr, "Unable to listen TCP connection\n");
        modbus_free(ctx);
        return -1;
    }

    /* Clear the reference set of socket */
    FD_ZERO(&ref_rdset);
    /* Add the server socket */
    FD_SET(server_socket, &ref_rdset);

    /* Keep track of the max file descriptor */
    fdmax = server_socket;

    while(1) {
        rdset = ref_rdset;

        if (select(fdmax + 1, &rdset,NULL, NULL, NULL) == -1) {
            perror("realtime wave data Server select() failure.");
            if (server_socket != -1) {
                close(server_socket);
            }
            modbus_free(ctx);
        }

        /* Run through the existing connections looking for data to be
         * read */
        for (master_socket = 0; master_socket <= fdmax; master_socket++) {

            if (!FD_ISSET(master_socket, &rdset) ) {
                continue;
            }

            if (master_socket == server_socket) {
                /* A client is asking a new connection */
                socklen_t addrlen;
                struct sockaddr_in clientaddr;
                int newfd;
                if(client_socket != -1 && connected){
                    setRealtimeSyncSendFlag(REALTIME_WAVE_SEND_STOP);
                    close(client_socket);
                    connected = false;
                    printf("realtime wave data Server disconnect old connection and establish new one!\n");
                    /* Remove from reference set */
                    FD_CLR(client_socket, &ref_rdset);
                    
                    if (client_socket == fdmax) {
                        fdmax--;
                    }                    
                }
                             
                /* Handle new connections */
                addrlen = sizeof(clientaddr);
                memset(&clientaddr, 0, sizeof(clientaddr));
                newfd = accept(server_socket, (struct sockaddr *) &clientaddr, &addrlen); 
                client_socket = newfd;
                if (newfd == -1) {
                    perror("realtime wave data Server accept() error");
                } else {
                    connected = true;
                    FD_SET(newfd, &ref_rdset);
                    if (newfd > fdmax) {
                        /* Keep track of the maximum */
                        fdmax = newfd;
                    }
                    setsockopt(client_socket, SOL_SOCKET, SO_SNDTIMEO, (void*)&timeout, sizeof(timeout));
                    printf("realtime wave data Server New connection from %s:%d on socket %d\n",
                           inet_ntoa(clientaddr.sin_addr),
                           clientaddr.sin_port,
                           newfd);
                }
            } else {
                modbus_set_socket(ctx, master_socket);
                //rc = modbus_receive(ctx, query);
                int sockfd = modbus_get_socket(ctx);
                client_socket = sockfd; 
                rc = recv(sockfd, query,MODBUS_TCP_MAX_ADU_LENGTH,0);
                if (rc > 0) {
                    printf("[%s][%d]: data handle\n", __func__,__LINE__);
                    handle_client_request(ctx, query, rc);
                    
                } else if (rc == -1) {
                    /* This example server in ended on connection closing or
                     * any errors. */
                    connected = false;
                    printf("realtime wave data Server Connection closed on socket %d\n", master_socket);
                    setRealtimeSyncSendFlag(REALTIME_WAVE_SEND_STOP);
                    close(master_socket);

                    /* Remove from reference set */
                    FD_CLR(master_socket, &ref_rdset);
                    
                    if (master_socket == fdmax) {
                        fdmax--;
                    }
                }
            }
            usleep(10000); //10ms
        }
    }

    return 0;    
}

int PACTable_ReadAll(uint8_t* pBuff)
/* 该函数用于读取PAC表中所有数据(注意:buff里的数据格式和自定义modbus命令相关的) */
{
    int onePacAddrDataByteQty=0;
    uint16_t pacIndex=0;

    printf("[%s]: data handle\n", __func__);


    for(pacIndex=0; pacIndex<ARM_MAX_LENGTH; pacIndex++)
    {

        /* 地址[2 bytes] */
        pBuff[onePacAddrDataByteQty++] = (uint8_t)((pacIndex >> 8) & 0xFF); // 保存高 8 位
        pBuff[onePacAddrDataByteQty++] = (uint8_t)(pacIndex & 0xFF); // 保存低 8 位

        /* 值 */
        uint16_t value = 0;
        UpperTable_Get((uint32_t)pacIndex, &value);

        /* 将arm小端字节序转换成modbus大端字节序 */
        pBuff[onePacAddrDataByteQty++] = (uint8_t)((value >> 8) & 0xFF); // 保存高 8 位到第一个字节
        pBuff[onePacAddrDataByteQty++] = (uint8_t)(value & 0xFF); // 保存低 8 位到第二个字节

    }
    /* 返回总字节数 */
    return onePacAddrDataByteQty;
}

void process_request(modbus_t *ctx, uint8_t *req ,uint8_t *rsp, response_callback callback) {
    // 调用回调函数，将处理结果传递给回调函数
    callback(ctx, req, rsp);
}
int transLog_handle_response(modbus_t *ctx, uint8_t *req ,uint8_t *rsp) {

    uint16_t respLength=0;
    LogClassData logClassData;
    logClassData.logType = upload_log;
    /* 子功能码 */
    uint8_t subFunCode=0;
    rsp[7]=req[7];/* 功能码 */
    rsp[8]=req[8];/* 子功能码 */

    subFunCode=req[8];
    for(int i = 0;i<9;i++){
        printf("[%s] [%d]: data handle subFunCode is %d\n", __func__ , __LINE__,req[i]);        
    }

    switch(subFunCode){
        case 1:{
            //上位机所需日志
            respLength = queryByTypes_db(&logClassData,&rsp[9]);
        }break;

        case 2:{
            //查询日志
            uint8_t time[4];
            for(int i = 0;i<4;i++){
                time[i] = req[i+9];
            }
            respLength = queryByTime_db(time, &rsp[9]);
        }break;

        case 3:{
            //待添加                   
        }break;

        case 4:{
            //待添加   
        }break;

        default:
            printf("不支持的子功能码");
            break;    
    }
    printf("[%s] [%d]: data handle respLength is %d\n", __func__ , __LINE__,respLength);    
        rsp[4] = (respLength + 3) >> 8;
        rsp[5] = (respLength + 3) & 0xff;

        respLength += 9;
    int sock = modbus_get_socket(ctx);
    if( !send(sock,rsp,respLength,0) )
    {
        printf("MSH(%d) send ERROR\n",sock);
        return -1;
    }
    return 0;

}

int operatePAC_handle_response(modbus_t *ctx, uint8_t *req ,uint8_t *rsp) {

    uint16_t respLength=0;
    /* 子功能码 */
    uint8_t subFunCode=0;
    rsp[7]=req[7];/* 功能码 */
    rsp[8]=req[8];/* 子功能码 */

    subFunCode=req[8];
    for(int i = 0;i<9;i++){
        printf("[%s] [%d]: data handle subFunCode is %d\n", __func__ , __LINE__,req[i]);        
    }

    switch(subFunCode){
        case 1:{
            //随意读，暂不支持,协议问题问题
        }break;

        case 2:{
            /*随意写 PAC地址 */
            uint16_t addr=(req[9]<<8)|(req[10]);
            uint16_t value= (req[11]<<8)|(req[12]);
            UpperTable_Set(addr,value);
        }break;

        case 3:{
            //读取变化所有值，目前点表暂未维护变化值
            printf("读取所有具有读权限的ARM/FPGA参数.\n");
            respLength = PACTable_ReadAll(&rsp[9]);                     
        }break;

        case 4:{
            printf("读取所有具有读权限的ARM/FPGA参数.\n");
            respLength = PACTable_ReadAll(&rsp[9]);
        }break;

        case 5:{
            printf("读取所有具有读权限的ARM/FPGA参数.\n");
            respLength = PACTable_ReadAll(&rsp[9]);
        }break;

        default:
        printf("不支持的子功能码");
            break;    
    }
        rsp[4] = (respLength + 3) >> 8;
        rsp[5] = (respLength + 3) & 0xff;

        respLength += 9;
    int sock = modbus_get_socket(ctx);
    if( !send(sock,rsp,respLength,0) )
    {
        printf("MSH(%d) send ERROR\n",sock);
        return -1;
    }
    return 0;
}

int realtime_wave_handle_response(modbus_t *ctx, uint8_t *req ,uint8_t *rsp) {

    /* 子功能码 */
    uint8_t subFunCode=0;
    rsp[7]=req[7];/* 功能码 */
    rsp[8]=req[8];/* 子功能码 */

    subFunCode=req[8];
    for(int i = 0;i<9;i++){
        printf("[%s] [%d]: data handle subFunCode is %d\n", __func__ , __LINE__,req[i]);        
    }

    switch(subFunCode){
        case 0:{
            //关闭实时波形发送
            setRealtimeSyncSendFlag(REALTIME_WAVE_SEND_STOP);
            printf("关闭实时波形发送\n");
        }break;

        case 9:{
            //开启实时波形发送
            setRealtimeSyncSendFlag(REALTIME_WAVE_SEND_START);
            setRealtimeSyncSendSocketFd(modbus_get_socket(ctx)); 
            printf("开启实时波形发送\n");
        }break;

        default:
            printf("不支持的子功能码\n");
            break;    
    }
    return 0;
}

int error_log_handle_response(modbus_t *ctx, uint8_t *req ,uint8_t *rsp) {

    uint16_t respLength=0;

    /* 子功能码 */
    uint8_t subFunCode;
    rsp[7]=req[7];/* 功能码 */
    rsp[8]=req[8];/* 子功能码 */

    subFunCode=req[8];
    for(int i = 0;i<10;i++){
        printf("[%s] [%d]: error wave data handle subFunCode is %d\n", __func__ , __LINE__,req[i]);        
    }

    switch(subFunCode){
        case 0:{
            //查询故障总信息rsp[9] rsp[10]是总数
            respLength = readErrorLogInfo_db(&rsp[9]);
        }break;

        case 1:{
            //读取那个故障信息 req[9] 读取第几个数据，req[9] - 1 算出偏移量0表示第一条1表示第二条，以此类推
            respLength = readSpecErrorLog_db(req[9],&rsp[9]);
        }break;

        case 2:{
            //读取头信息   
            respLength = readErrorLogHeadInfo(&rsp[9]); 
            for(int i = 0; i < respLength + 9; i++){
                printf("%x ", rsp[i]);
            }   
            printf("\n");            
        }break;

        case 3:{
            //读取参数信息 
            respLength = readErrorLogPACData(&rsp[9]);  
        }break;

        case 4:{
            //读取故障波形数据, which group index is gaven by req[9]
            respLength = readErrorGroupWave(req[9],&rsp[9]);  
        }break;

        default:
            printf("不支持的子功能码\n");
            break;    
    }
    printf("[%s] [%d]: error log data handle respLength is %d\n", __func__ , __LINE__,respLength);    
    rsp[4] = (respLength + 3) >> 8;
    rsp[5] = (respLength + 3) & 0xff;

    respLength += 9;
    int sock = modbus_get_socket(ctx);
    if( !send(sock,rsp,respLength,0) )
    {
        printf("MSH(%d) send ERROR\n",sock);
        return -1;
    }
    return 0;

}

int handle_client_request(modbus_t *ctx, uint8_t *req, int req_length) {

    int function;
    int ret = 0;
    uint16_t lengthValue = 0;
    if (ctx == NULL) {
        errno = EINVAL;
        return -1;
    }
  
    lengthValue = (req[4] << 8) + req[5];/* 长度 */
    function = req[7];
    printf("[%s][%d]: data req_length is %d lengthValue %d\n", __func__,__LINE__,req_length,lengthValue);
    if(req_length < (lengthValue))
    {
        printf("[handle_client_request] 帧长度不对.\n");
        return -1;
    }

    /* Data are flushed on illegal number of values errors. */
    switch (function) {

        case MODBUS_CUSTOM_FC_TransRealtimeWave: {
            uint8_t* rsp = (uint8_t*)malloc(5120 * sizeof(uint8_t));
            //MBAP
            rsp[0] = req[0];
            rsp[1] = req[1];
            rsp[2] = req[2];
            rsp[3] = req[3];
            rsp[6] = req[6];              
            printf("[%s]: data handle 64\n", __func__);
            process_request(ctx, req, rsp, realtime_wave_handle_response);
            ret = 0;
            free(rsp);
        }break;

        case MODBUS_CUSTOM_FC_TransLog:{
            uint8_t* rsp = (uint8_t*)malloc(130000 * sizeof(uint8_t));
            //MBAP
            rsp[0] = req[0];
            rsp[1] = req[1];
            rsp[2] = req[2];
            rsp[3] = req[3];
            rsp[6] = req[6];
            printf("[%s]: data handle 66\n", __func__);
            process_request(ctx, req, rsp, transLog_handle_response);
            free(rsp);
        }break;

        case MODBUS_CUSTOM_FC_FirmwareUpgrade:{

        }break;

        case MODBUS_CUSTOM_FC_FC_TransErrorlog:{
            uint8_t* rsp = (uint8_t*)malloc(5120 * sizeof(uint8_t));
            //MBAP
            rsp[0] = req[0];
            rsp[1] = req[1];
            rsp[2] = req[2];
            rsp[3] = req[3];
            rsp[6] = req[6];            
            //处理故障波形的发送
            printf("[%s]: data handle 6A\n", __func__);
            process_request(ctx, req, rsp, error_log_handle_response);
            ret = 0;
            free(rsp);
        }break;

        case MODBUS_CUSTOM_FC_OperatePAC:{
            uint8_t* rsp = (uint8_t*)malloc(4096 * sizeof(uint8_t));
            //MBAP
            rsp[0] = req[0];
            rsp[1] = req[1];
            rsp[2] = req[2];
            rsp[3] = req[3];
            rsp[6] = req[6];            
            printf("[%s]: data handle 6e\n", __func__);
            process_request(ctx, req, rsp, operatePAC_handle_response);
            ret = 0;
            free(rsp);
        }break;

        default: {
            ret = -1;
        }break;

    }

    return ret;

}